#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include<pcl/filters/passthrough.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=100118;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

Eigen::Matrix4f P2_, Trv2c_;

void pcd2bin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, string& out_file)
{
    //Create & write .bin file
//    ofstream bin_file(out_file.c_str(),ios::out|ios::binary|ios::app);
    ofstream bin_file(out_file.c_str());
    if(!bin_file.good()) cout<<"Couldn't open "<<out_file<<endl;

    //PCD 2 BIN
    cout << "Converting "<<endl;
    for (size_t i = 0; i < cloud->points.size (); ++i)
    {
        bin_file.write((char*)&cloud->points[i].x,3*sizeof(float));
        bin_file.write((char*)&cloud->points[i].intensity,sizeof(float));
        //cout<< 	cloud->points[i]<<endl;
    }

    bin_file.close();
}

void saveCalib(string& out_file)
{
    ofstream outfile;      //终端输入--》内存--》文本
    outfile.open(out_file.c_str());//(输入流) （变量）（输出文件流）
    if(!outfile) cout<<"calib error"<<endl;         

    outfile<<"P0: "<<0<<" "<<0<<" "<<0<<" "<<0<<" "
    <<0<<" "<<0<<" "<<0<<" "<<0<<" "
     <<0<<" "<<0<<" "<<0<<" "<<0<<std::endl;
    outfile<<"P1: "<<0<<" "<<0<<" "<<0<<" "<<0<<" "
    <<0<<" "<<0<<" "<<0<<" "<<0<<" "
     <<0<<" "<<0<<" "<<0<<" "<<0<<std::endl;
    outfile<<"P2: "<<P2_(0,0)<<" "<<P2_(0,1)<<" "<<P2_(0,2)<<" "<<P2_(0,3)<<" "
    <<P2_(1,0)<<" "<<P2_(1,1)<<" "<<P2_(1,2)<<" "<<P2_(1,3)<<" "
     <<P2_(2,0)<<" "<<P2_(2,1)<<" "<<P2_(2,2)<<" "<<P2_(2,3)<<std::endl;
    outfile<<"P3: "<<P2_(0,0)<<" "<<P2_(0,1)<<" "<<P2_(0,2)<<" "<<P2_(0,3)<<" "
    <<P2_(1,0)<<" "<<P2_(1,1)<<" "<<P2_(1,2)<<" "<<P2_(1,3)<<" "
     <<P2_(2,0)<<" "<<P2_(2,1)<<" "<<P2_(2,2)<<" "<<P2_(2,3)<<std::endl;
    outfile<<"R0_rect: "<<1<<" "<<0<<" "<<0<<" "
    <<0<<" "<<1<<" "<<0<<" "
     <<0<<" "<<0<<" "<<1<<std::endl;
    outfile<<"Tr_velo_to_cam: "<<Trv2c_(0,0)<<" "<<Trv2c_(0,1)<<" "<<Trv2c_(0,2)<<" "<<Trv2c_(0,3)<<" "
    <<Trv2c_(1,0)<<" "<<Trv2c_(1,1)<<" "<<Trv2c_(1,2)<<" "<<Trv2c_(1,3)<<" "
     <<Trv2c_(2,0)<<" "<<Trv2c_(2,1)<<" "<<Trv2c_(2,2)<<" "<<Trv2c_(2,3)<<std::endl;
    outfile<<"Tr_imu_to_velo: "<<1<<" "<<0<<" "<<0<<" "<<0.23<<" "
    <<0<<" "<<1<<" "<<0<<" "<<0<<" "
     <<0<<" "<<0<<" "<<1<<" "<<-0.133<<std::endl;

    outfile.close();
}

vector<int> project2image2dbbox(double h, double w, double l, double x, double y, double z, double yaw){
    MatrixXd xyz(3,8);
    xyz<<l / 2., l / 2., -l / 2., -l / 2., l / 2., l / 2., -l / 2., -l / 2.,
            0,0,0,0,-h,-h,-h,-h,
            w / 2., -w / 2., -w / 2., w / 2., w / 2., -w / 2., -w / 2., w / 2.;
//    cout<<"hwl"<<h<<" "<<w<<" "<<l<<endl;
//    xyz<<-w/2, -w/2,w/2,w/2,-w/2, -w/2,w/2,w/2,
//            -h,0,0,-h,-h,0,0,-h,
//            l/2,l/2,l/2,l/2,-l/2,-l/2,-l/2,-l/2;

    MatrixXd rotM (3,3);
    rotM<<cos(yaw), 0,sin(yaw),
            0, 1,0,
            -sin(yaw), 0, cos(yaw);
//    rotM=rotM.transpose();
    xyz = rotM*xyz;
    xyz(0,0) = xyz(0,0) + x;xyz(0,1) = xyz(0,1)  + x; xyz(0,2) = xyz(0,2) + x; xyz(0,3) = xyz(0,3) + x;
    xyz(0,4) = xyz(0,4) + x;xyz(0,5) = xyz(0,5) + x;xyz(0,6) = xyz(0,6) + x;xyz(0,7) = xyz(0,7) + x;
    xyz(1,0) = xyz(1,0) + y; xyz(1,1) = xyz(1,1) + y; xyz(1,2) = xyz(1,2) + y; xyz(1,3) = xyz(1,3) + y;
    xyz(1,4) = xyz(1,4) + y;xyz(1,5) = xyz(1,5) + y;xyz(1,6) = xyz(1,6) + y;xyz(1,7) = xyz(1,7) + y;
    xyz(2,0) = xyz(2,0) + z; xyz(2,1) = xyz(2,1) + z; xyz(2,2) = xyz(2,2) + z; xyz(2,3) = xyz(2,3) + z;
    xyz(2,4) = xyz(2,4) + z;xyz(2,5) = xyz(2,5) + z;xyz(2,6) = xyz(2,6) + z;xyz(2,7) = xyz(2,7) + z;


    MatrixXd P2 (3,4);
    P2<<384.1054992675781, 0.0, 320.1566147348167,0.0,
         0.0, 384.8056640625, 237.8698444374022, 0.0, 
         0.0, 0.0, 1.0, 0.0;

    MatrixXd xyz_new_M (4,8);
    xyz_new_M<<xyz(0,0), xyz(0,1),xyz(0,2),xyz(0,3),xyz(0,4),xyz(0,5),xyz(0,6),xyz(0,7),
            xyz(1,0), xyz(1,1),xyz(1,2),xyz(1,3),xyz(1,4),xyz(1,5),xyz(1,6),xyz(1,7),
            xyz(2,0), xyz(2,1),xyz(2,2),xyz(2,3),xyz(2,4),xyz(2,5),xyz(2,6),xyz(2,7),
            1,1,1,1,1,1,1,1;
//    cout<<"my debug bbox"<<xyz_new_M<<endl;
    xyz=P2*xyz_new_M;

    vector<double> x_image,y_image;
    for(int i=0; i<8; i++){
        x_image.push_back(xyz(0,i)/xyz(2,i));
        y_image.push_back(xyz(1,i)/xyz(2,i));
//        cout<<"x:"<<xyz(0,i)/xyz(2,i)<<endl;
//        cout<<"y:"<<xyz(1,i)/xyz(2,i)<<endl;
    }


    vector<int> bbox;
    bbox.push_back(int(*min_element(x_image.begin(),x_image.end())));
    bbox.push_back(int(*min_element(y_image.begin(),y_image.end())));
    bbox.push_back(int(*max_element(x_image.begin(),x_image.end())));
    bbox.push_back(int(*max_element(y_image.begin(),y_image.end())));
    if(bbox[0]<0)
        bbox[0]=0;
    if(bbox[1]<0)
        bbox[1]=0;
    if(bbox[2]>640)
        bbox[2]=640;
    if(bbox[3]>480)
        bbox[3]=480;
//    cout<<"my debug bbox"<<bbox[0]<<" "<<bbox[1]<<" "<<bbox[2]<<" "<<bbox[3]<<endl;
    return bbox;
}

void saveLabel(string& out_file, vector<vector<double>> label)
{
    ofstream outfile;      //终端输入--》内存--》文本
    outfile.open(out_file.c_str());//(输入流) （变量）（输出文件流）
    if(!outfile) cout<<"label error"<<endl;         

    int object_num=label.size();
    vector<int> bbox_image;
    for(int i=0; i<object_num; i++){

        bbox_image=project2image2dbbox(label[i][0],label[i][1],label[i][2],label[i][3],label[i][4],label[i][5],label[i][6]);

        // outfile<<"Car "<<0<<" "<<0<<" "<<0<<" "
        // <<bbox_image[0]<<" "<<bbox_image[1]<<" "<<bbox_image[2]<<" "<<bbox_image[3]<<" "  //2dbbbox
        // <<label[i][0]<<" "<<label[i][1]<<" "<<label[i][2]<<" " //hwl
        // <<label[i][3]<<" "<<label[i][4]<<" "<<label[i][5]<<" " //xyz
        // <<label[i][6]<<" "<<0<<std::endl;  //yaw

        string class_name;
        if(label[i][7]<=1)
            class_name = "Car ";
        else if(label[i][7]==2)
            class_name = "Pedestrian ";

        outfile<<class_name
        <<label[i][3]<<" "<<label[i][4]<<" "<<label[i][5] //xyz
       <<std::endl;  //yaw
    }
    outfile.close();
}

void saveImgLabel(string& out_file, vector<vector<double>> label, string img_path, cv::Mat img)
{
    ofstream outfile;      //终端输入--》内存--》文本
    outfile.open(out_file.c_str(), ios::out|ios::binary|ios::app);//(输入流) （变量）（输出文件流）
    if(!outfile) cout<<"img label error"<<endl;         
    // outfile<<img_path<<" ";

    int object_num=label.size();
    vector<int> bbox_image;
    int center_x, center_y;
    for(int i=0; i<object_num; i++){
        {
            Eigen::Vector3f pt;
            pt = (P2_*Eigen::Vector4f(label[i][3], label[i][4] - label[i][0]/2, label[i][5], 1)).topRows(3);
            int32_t u = static_cast<int32_t>(pt(0)/pt(2));
            int32_t v = static_cast<int32_t>(pt(1)/pt(2));
            // cout<<"center x y "<<u<<" "<<v<<endl;
            if (u>=0 && u<= 640 && v >= 0 && v <= 480 ){
                center_x = u;
                center_y = v;
            }else{
                continue;
            }
        }
        if(label[i][7] == 0)
            circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(255,0, 0), 4);
        if(label[i][7]  == 1)
            circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(0,255, 0), 4);
        if(label[i][7]  == 2)
            circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(255,255, 0), 4);

        // outfile<<center_x<<","<<center_y<<","<<i<<" ";
        outfile<<i<<" "<<float(center_x)/640<<" "<<float(center_y)/480<<" "<<0.01<<" "<<0.01;
    }
    outfile<<endl;
    outfile.close();
    cv::imshow("img_center", img);
    cv::waitKey(500);
}


void saveImgLabelYolo(string& out_file, vector<vector<double>> label, string img_path, cv::Mat img)
{
    ofstream outfile;      //终端输入--》内存--》文本
    outfile.open(out_file.c_str());//(输入流) （变量）（输出文件流）
    if(!outfile) cout<<"img label yolo error"<<endl;         

    int object_num=label.size();
    vector<int> bbox_image;
    int center_x, center_y;
    for(int i=0; i<object_num; i++){
        {
            Eigen::Vector4f obj_center =  Eigen::Vector4f(label[i][3], label[i][4], label[i][5], 1);
            obj_center = (Trv2c_*obj_center).eval();
            Eigen::Vector3f pt;
            pt = (P2_*obj_center).topRows(3);
            int32_t u = static_cast<int32_t>(pt(0)/pt(2));
            int32_t v = static_cast<int32_t>(pt(1)/pt(2));
            // cout<<"center x y "<<u<<" "<<v<<endl;
            if (u>=0 && u<= 640 && v >= 0 && v <= 480 ){
                center_x = u;
                center_y = v;
            }else{
                continue;
            }
        }
        if(label[i][7] == 0)
            circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(255,0, 0), 4);
        if(label[i][7]  == 1)
            circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(0,255, 0), 4);
        if(label[i][7]  == 2)
            circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(255,255, 0), 4);

        // outfile<<center_x<<","<<center_y<<","<<i<<" ";
        outfile<<i<<" "<<float(center_x)/640<<" "<<float(center_y)/480<<" "<<0.01<<" "<<0.01<<endl;
    }
    outfile.close();
    cv::imshow("img_center", img);
    cv::waitKey(500);
}


void saveTrajectory(string& out_file,double time_stamp, double x,double y,double z,double ow,double ox,double oy,double oz, double vx,double vy,double vz)
{
    ofstream outfile(out_file.c_str(),ios::out|ios::binary|ios::app);
    if(!outfile) cout<<"trajectory error"<<endl;
    outfile<<time_stamp<<" "<<x<<" "<<y<<" "<<z<<" "    //xyz
           <<ow<<" "<<ox<<" "<<oy<<" "<<oz<<" "<<vx<<" "<<vy<<" "<<vz<<std::endl;  //
    outfile.close();
}


void saveIndex(string& out_file, string index)
{
    ofstream outfile;      //终端输入--》内存--》文本
    outfile.open(out_file.c_str(), ios::out|ios::binary|ios::app);//(输入流) （变量）（输出文件流）
    if(!outfile) cout<<"index error"<<endl;         

    outfile<<index;

    outfile<<endl;
    outfile.close();
}

void cloudCB(vector<string> label_path, vector<string> pc_path, vector<string> img_path)
{
    Eigen::Vector4f obj_center_;

    for(int label_idx = 0; label_idx < label_path.size(); label_idx++){
        frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                        "%06d", frame_num_));
        pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
        pcl::io::loadPCDFile<PointType>(pc_path[label_idx], *pc_tmp);
    
        string root_path = "/media/st/data_fast/keti/0826/";

        string tranin_or_test="training";
        // string tranin_or_test="testing";
        string dataset_name = "center_label";
        cout<<tranin_or_test<<endl;

        for(auto & pt:pc_tmp->points){
            pt.intensity = 0;
        }

        pcl::PassThrough<PointType> pass_filter;
        pass_filter.setFilterFieldName("x");
        pass_filter.setFilterLimits(0,20);
        pass_filter.setFilterLimitsNegative(false);
        pass_filter.setInputCloud(pc_tmp);
        pass_filter.filter(*pc_tmp);
        pass_filter.setFilterFieldName("y");
        pass_filter.setFilterLimits(-10,10);
        pass_filter.setFilterLimitsNegative(false);
        pass_filter.setInputCloud(pc_tmp);
        pass_filter.filter(*pc_tmp);


        pc_path_name_=root_path+dataset_name+"/"+tranin_or_test+"/velodyne/"+frame_id_string_+".bin";
        pcd2bin(pc_tmp,pc_path_name_);

        ifstream read_label;
        read_label.open(label_path[label_idx]);
        string s_line,class_,truncated_,dx_,dy_,dz_,x_,y_,z_,theta_,ahpha_;
        vector<vector<double>> label;
        while(getline(read_label,s_line)){
            istringstream sin(s_line);
            sin>>class_>>truncated_>>dx_>>dy_>>dz_>>x_>>y_>>z_>>theta_>>ahpha_;
            vector<double> label_this(8);
            label_this[0] = atof(dz_.c_str());
            label_this[1] = atof(dy_.c_str());
            label_this[2] = atof(dx_.c_str());
            obj_center_(0)=atof(x_.c_str());
            obj_center_(1)=atof(y_.c_str());
            obj_center_(2)=atof(z_.c_str());
            obj_center_(3)=1;
            // obj_center_ = T_camera_lidar_*obj_center_;
            label_this[3] = obj_center_(0);
            label_this[4] = obj_center_(1);
            label_this[5] = obj_center_(2);
            label_this[6] = atof(ahpha_.c_str())/180*M_PI;
            if(label_this[6]>=0&&label_this[6]<=M_PI/2){
                label_this[6]=label_this[6]+M_PI/2;
            }
            else if(label_this[6]>M_PI/2&&label_this[6]<=M_PI){
                label_this[6]=label_this[6]-3*M_PI/2;
            }
            else if(label_this[6]<-M_PI/2&&label_this[6]>=-M_PI){
                label_this[6]=label_this[6]+M_PI/2;
            }
            else if(label_this[6]<0&&label_this[6]>=-M_PI/2){
                label_this[6]=label_this[6]+M_PI/2;
            }
            label_this[6]=-label_this[6];

            //class
            if(class_=="Car"){
                label_this[7] = 0;
            }
            if(class_=="Cart"){
                label_this[7] = 1;
            }
            if(class_=="Pedestrian"){
                label_this[7] = 2;
            }

            label.push_back(label_this);
        }

        pc_path_name_= root_path+dataset_name+"/"+tranin_or_test+"/label_2/"+frame_id_string_+".txt";
        saveLabel(pc_path_name_,label);

        // cv::Mat img = cv::imread(img_path[label_idx]);
        // // cv::imwrite("/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png",img);
        // string img_center_path_name_;
        // img_center_path_name_=root_path+dataset_name+"/"+tranin_or_test+"/"+"img_center_label.txt";
        // saveImgLabel(img_center_path_name_, label, img_path[label_idx], 
        //                            img );

        cv::Mat img = cv::imread(img_path[label_idx]);
        cv::imwrite(root_path+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png",img);
        string img_center_path_name_;
        img_center_path_name_=root_path+dataset_name+"/"+tranin_or_test+"/image_label/"+frame_id_string_+".txt";
        saveImgLabelYolo(img_center_path_name_, label, img_path[label_idx], 
                                   img );

        pc_path_name_=root_path+dataset_name+"/"+tranin_or_test+"/calib/"+frame_id_string_+".txt";
        saveCalib(pc_path_name_);

        string index_path_name_;
        index_path_name_=root_path+dataset_name+"/"+tranin_or_test+"/"+"index.txt";
        saveIndex(index_path_name_,  frame_id_string_);
        frame_num_++;
    }
}

vector<string> getFiles(string cate_dir)
{
    vector<string> files; //存放文件名

    DIR *dir;
    struct dirent *ptr;
    char base[1000];

    if ((dir = opendir(cate_dir.c_str())) == NULL)
    {
        perror("Open dir error...");
        exit(1);
    }

    while ((ptr=readdir(dir)) != NULL)
	{
		if(strcmp(ptr->d_name,".")==0 || strcmp(ptr->d_name,"..")==0)    ///current dir OR parrent dir
            continue;
        else if(ptr->d_type == 8)    ///file
			//printf("d_name:%s/%s\n",basePath,ptr->d_name);
			files.push_back(ptr->d_name);
		else if(ptr->d_type == 10)    ///link file
			//printf("d_name:%s/%s\n",basePath,ptr->d_name);
			continue;
		else if(ptr->d_type == 4)    ///dir
		{
			files.push_back(ptr->d_name);
			/*
		        memset(base,'\0',sizeof(base));
		        strcpy(base,basePath);
		        strcat(base,"/");
		        strcat(base,ptr->d_nSame);
		        readFileList(base);
			*/
		}
	}
	closedir(dir);
 
	//排序，按从小到大排序
	sort(files.begin(), files.end());
	return files;
}

int main(int argc, char* argv[])
{
//    tf2_ros::TransformListener tfl_(tfbuf_);
//    frame_ids_["odom"]=std::string("odom");
//    frame_ids_["base_link"]=std::string("base_link");
//    frame_ids_["map"]=std::string("map");
//    cloud_pub=nh.advertise<sensor_msgs::PointCloud2>("cloud_accum_convert",10,true);
//    ros::Subscriber cloud_sub=nh.subscribe("/cloud",10,cloudCB);

    P2_ = Eigen::Matrix4f::Identity();
    float p2[12] = {384.1054992675781, 0.0, 320.1566147348167, 0.0,
         0.0, 384.8056640625, 237.8698444374022, 0.0,
         0.0, 0.0, 1.0, 0.0};
    Trv2c_ = Eigen::Matrix4f::Identity();
    float trv2c[12] = {-0.0178848,   -0.999827,  -0.0051127,  0.1,
-0.00237361,  0.00515596,   -0.999984,  -0.0549754,
   0.999837,  -0.0178723, -0.00246541,   -0.129242};
    for (int i = 0; i<12; i++){
        P2_(i/4, i%4) = p2[i];
        Trv2c_(i/4, i%4) = trv2c[i];
    }

    string img_path_m = "/media/st/data_fast/keti/0826/lidar_annotation/image_2";
    string pc_path_m = "/media/st/data_fast/keti/0826/lidar_annotation/_pcd";
    string label_path_m = "/media/st/data_fast/keti/0826/lidar_annotation/_bbox";

    vector<string> img_path = getFiles(img_path_m);
    vector<string> pc_path = getFiles(pc_path_m);
    vector<string> label_path = getFiles(label_path_m);
    
    for(auto &path:img_path){
        path = img_path_m+"/"+path;
        cout<<path<<endl;
    }
    for (auto &path : pc_path){
        path = pc_path_m+"/"+path;
        cout<<path<<endl;
    }
    for (auto &path : label_path){
        path = label_path_m+"/"+path;
        cout<<path<<endl;
    }

    cloudCB(label_path,pc_path, img_path);

    return 0;
}
